#include <grass/gis.h>
#include "bouman.h"
#include "region.h"


static void decimate (LIKELIHOOD ***,struct Region *,int,LIKELIHOOD ***,double);
static void up_ll (LIKELIHOOD *,int,double,LIKELIHOOD *);


void make_pyramid (
    LIKELIHOOD ****ll_pym,  /* log likelihood pyramid, ll_pym[scale][i][j][class] */
    struct Region *region,   /* specifies image subregion */
    int M,                  /* number of classes */
    double *alpha,          /* decimation parameters */
    int vlevel             /* verbose operation */
)
{
    int  D;
    int  wd,ht;
    struct Region region_buff;

    /* save region stucture */
    copy_reg(region,&region_buff);

    D=0; 
    reg_to_wdht(region,&wd,&ht);
    while((wd>2)&&(ht>2)) {
      if(vlevel>=2)
        G_message("D = %d  alpha = %f; 1-alpha = %f", D,alpha[D],1-alpha[D]);
      decimate(ll_pym[D],region,M,ll_pym[D+1],alpha[D]);
      dec_reg(region);
      reg_to_wdht(region,&wd,&ht);
      D++;
    }

    /* restore region structure */
    copy_reg(&region_buff,region);
}

static void decimate (
/* decimate statistics ll1 to form ll2 */
    LIKELIHOOD ***ll1,     /* log likelihood ll1[i][j][class], at fine resolution */
    struct Region *region1,/* specifies image subregion */
    int M,                 /* number of classes */
    LIKELIHOOD ***ll2,     /* loglikelihood ll1[i][j][class], at coarse resolution*/
    double alpha          /* transition parameter */
)
{
    struct Region *region2,reg_spc; /* coarse resolution region */
    int  wflag,hflag;               /* flags indicate odd number of pixels */
    int	 i,j,m;
    LIKELIHOOD *node;               /* coarse resolution point */
    LIKELIHOOD *pt1,*pt2,*pt3,*pt4; /* fine resolution neighbors */

    region2 = &reg_spc;

    copy_reg(region1,region2);
    dec_reg(region2);

    wflag = region1->xmax&1;
    hflag = region1->ymax&1;

    for(i=region2->ymin; i<region2->ymax; i++)
    for(j=region2->xmin; j<region2->xmax; j++) {
      pt1= ll1[2*i][2*j];
      pt2= ll1[2*i][2*j+1];
      pt3= ll1[2*i+1][2*j];
      pt4= ll1[2*i+1][2*j+1];

      node= ll2[i][j];
      for(m=0; m<M; m++) node[m] = 0.0;

      up_ll(pt1,M,alpha,node);
      up_ll(pt2,M,alpha,node);
      up_ll(pt3,M,alpha,node);
      up_ll(pt4,M,alpha,node);
    }

    if(wflag) {
      for(i=region2->ymin; i<region2->ymax; i++) {
        node = ll2[i][region2->xmax-1];
        for(m=0; m<M; m++) node[m] = 0.0;

        pt1 = ll1[2*i][region1->xmax-1];
        pt2 = ll1[2*i+1][region1->xmax-1];
        up_ll(pt1,M,alpha,node);
        up_ll(pt2,M,alpha,node);
      }
    }

    if(hflag) {
      for(j=region2->xmin; j<region2->xmax; j++) {
        node = ll2[region2->ymax-1][j];
        for(m=0; m<M; m++) node[m] = 0.0;

        pt1 = ll1[region1->ymax-1][2*j];
        pt2 = ll1[region1->ymax-1][2*j+1];
        up_ll(pt1,M,alpha,node);
        up_ll(pt2,M,alpha,node);
      }
    }

    if(hflag&&wflag) {
      node = ll2[region2->ymax-1][region2->xmax-1];
      for(m=0; m<M; m++) node[m] = 0.0;

      pt1 = ll1[region1->ymax-1][region1->xmax-1];
      up_ll(pt1,M,alpha,node);
    }
}


static void up_ll (
    LIKELIHOOD *pt1, /* array of log likelihood values, pt1[class] */
    int M,
    double alpha,
    LIKELIHOOD *pt2 /* array of log likelihood values, pt2[class] */
)
{
    static int m;
    static double sum,max,cprob[256];
 
    if(alpha!=1.0) {
      max = pt1[0];
      for(m=1; m<M; m++) 
        if(pt1[m]>max) max= pt1[m];

      sum = 0;
      for(m=0; m<M; m++) {
        cprob[m] = exp(pt1[m]-max);
        sum += cprob[m];
      }
 
      sum = ((1-alpha)*sum)/M;
 
      for(m=0; m<M; m++)
        pt2[m] += log( sum + alpha*cprob[m]) + max;
    }
    else {
      for(m=0; m<M; m++) pt2[m] += pt1[m];
    }
}

char ***get_pyramid (int w0, int h0, size_t size)
{
    char ***pym;
    int  D,wn,hn;

    D = levels(w0,h0);
    pym=(char ***)G_malloc((D+1)*sizeof(char *));
 
    D=0; wn = w0; hn = h0;
    pym[D]=(char **)get_img(wn,hn,size);
    while((wn>2)&&(hn>2)) {
      D++; 
      wn = wn/2;
      hn = hn/2;
      pym[D]=(char **)get_img(wn,hn,size);
    }

    return(pym);
}


void free_pyramid (char *pym, int wd, int ht)
{
    unsigned char ***pt;
    int  i,D;

    pt = (unsigned char ***)pym;
    D=levels(wd,ht);
    for(i=0; i<=D; i++) free_img(pt[i]);
    G_free(pym);
}

char ****get_cubic_pyramid (int w0, int h0, int M, size_t size)
{
    char ****pym;
    int  D,wn,hn;

    D = levels(w0,h0);
    pym=(char ****)G_malloc((D+1)*sizeof(char *));
 
    D=0; wn = w0; hn = h0;
    pym[D]=(char ***)multialloc(size,3,hn,wn,M);
    while((wn>2)&&(hn>2)) {
      D++; 
      wn = wn/2;
      hn = hn/2;
      pym[D]=(char ***)multialloc(size,3,hn,wn,M);
    }

    return(pym);
}


void free_cubic_pyramid (char *pym, int wd, int ht, int M)
{
    int  i,D;
    char **pt;

    pt = (char **)pym;
    D=levels(wd,ht);
    for(i=0; i<=D; i++) multifree(pt[i],M);
    G_free(pym);
}

int levels (int wd, int ht) 
{
    int D;

    D=0;
    while((wd>2)&&(ht>2)) {
      D++;
      wd = wd/2;
      ht = ht/2;
      }
    return(D);
}
